from pyb import LED
import os
import sensor, image, time
import openmv_numpy as np



sensor.reset()

sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
sensor.set_brightness(1500) # 设置图像亮度 越大越亮
sensor.skip_frames(time = 20)
sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
clock = time.clock()

white = LED(4)  # 定义一个LED4   照明灯


def GetTransformM(A, B):
    """
    获取变换矩阵 A 是原坐标点 B 是变化后的坐标点\n
    例子：\n
    A_ = [[75, 0], [137, 0], [22, 89], [189, 89]]\n
    B_ = [[41, 0], [158, 0], [41, 89], [158, 89]]\n
    res = GetTransformM(A_, B_)\n
    """
    mata = np.array([
        [A[0][0], A[0][1], 1, 0, 0, 0, -A[0][0] * B[0][0], -A[0][1] * B[0][0]],
        [0, 0, 0, A[0][0], A[0][1], 1, -A[0][0] * B[0][1], -A[0][1] * B[0][1]],
        [A[1][0], A[1][1], 1, 0, 0, 0, -A[1][0] * B[1][0], -A[1][1] * B[1][0]],
        [0, 0, 0, A[1][0], A[1][1], 1, -A[1][0] * B[1][1], -A[1][1] * B[1][1]],
        [A[2][0], A[2][1], 1, 0, 0, 0, -A[2][0] * B[2][0], -A[2][1] * B[2][0]],
        [0, 0, 0, A[2][0], A[2][1], 1, -A[2][0] * B[2][1], -A[2][1] * B[2][1]],
        [A[3][0], A[3][1], 1, 0, 0, 0, -A[3][0] * B[3][0], -A[3][1] * B[3][0]],
        [0, 0, 0, A[3][0], A[3][1], 1, -A[3][0] * B[3][1], -A[3][1] * B[3][1]],
    ])

    matb = np.array([[B[0][0]],[B[0][1]],[B[1][0]],[B[1][1]],
                     [B[2][0]],[B[2][0]],[B[3][0]],[B[3][1]]])

    val = np.solve(mata, matb)

    val = np.array([
        [val[0][0], val[1][0], val[2][0]],
        [val[3][0], val[4][0], val[5][0]],
        [val[6][0], val[7][0], 1.0],
    ])

    #return val.inv()
    return val


B = [[0,0],[159,0],[159,119],[0,119]]

def TransformTest(cood,m):
    maxx=159
    maxy=119

    cood = np.array([[x],[y],[1]])
    tmp = m * cood
    newx = tmp[0][0]/tmp[2][0]
    newy = tmp[1][0]/tmp[2][0]
    return res


while(True):
    img = sensor.snapshot()

    img.binary([(255,140)])

    for r in img.find_rects(threshold = 20000):
        img.draw_rectangle(r.rect(),color = (255,0,0))
        #m = GetTransformM(r.corners(),B)
        for c in img.find_circles(threshold = 3500, x_margin = 0, y_margin = 0, r_margin = 0,r_min = 1, r_max = 5, r_step = 2):
            img.draw_circle(c.x(), c.y(), c.r(), color = 125)















